6D SLAM with Cached kd-tree Search

6D SLAM (Simultaneous Localization and Mapping) or 6D Concurrent Localization and Mapping of mobile robots considers six degrees of freedom for the robot pose, namely, the x, y and z coordinates and the roll, yaw and pitch angles. In previous work we presented our scan matching based 6D SLAM approach [10–12,16], where scan matching is based on the well known iterative closest point (ICP) algorithm [3]. Efficient implementations of this algorithm are a result of a fast computation of closest points. The usual approach, i.e., using kd-trees is extended in this paper. We describe a novel search stategy, that leads to significant speed-ups. Our mapping system is real-time capable, i.e., 3D maps are computed using the resources of the used Kurt3D robotic hardware.